#include "cvxCamera.h"
#include "cvxLog.h"
#include "cvxMat.h"
#include "cvxGeometry.h"

/************************************************************************/
/*  CvxCamera                                                           */
/************************************************************************/
CvxCamera::CvxCamera()
{
	m_intrinsic   = Mat::eye(3, 3, CV_64F);
	m_distortion  = Mat::zeros(5, 1, CV_64F);
	m_imageSize = cvSize(1920, 1080);
}

bool CvxCamera::write(FILE *pf)
{
	assert(pf);

	//focal length
	float fx = m_intrinsic.at<double>(0, 0);
	float fy = m_intrinsic.at<double>(1, 1);
	fprintf(pf, "%f %f ", fx, fy);

	//principla point
	float cx = m_intrinsic.at<double>(0, 2);
	float cy = m_intrinsic.at<double>(1, 2);
	fprintf(pf, "%f %f ", cx, cy);

	//distortion
	float k1, k2, p1, p2, k3;
	k1 = m_distortion.at<double>(0, 0);
	k2 = m_distortion.at<double>(1, 0);
	p1 = m_distortion.at<double>(2, 0);
	p2 = m_distortion.at<double>(3, 0);
	k3 = m_distortion.at<double>(4, 0);
	fprintf(pf, "%f %f %f %f %f\n", k1, k2, p1, p2, k3);
	return true;
}

bool CvxCamera::read(FILE *pf)
{
	assert(pf);
	char line[256] = {NULL};
	if (fgets(line, 256, pf))
	{
		float fx = -1.0, fy, cx, cy;
		float k1, k2, p1, p2, k3;
		if(sscanf(line, "%f %f %f %f %f %f %f %f %f", &fx, &fy, &cx, &cy, &k1, &k2, &p1, &p2, &k3) == 9)
		{
			assert(fx != -1.0);
			m_intrinsic.at<double>(0, 0) = fx;
			m_intrinsic.at<double>(1, 1) = fy;
			
			m_intrinsic.at<double>(0, 2) = cx;
			m_intrinsic.at<double>(1, 2) = cy;

			m_distortion.at<double>(0, 0) = k1;
			m_distortion.at<double>(1, 0) = k2;
			m_distortion.at<double>(2, 0) = p1;
			m_distortion.at<double>(3, 0) = p2;
			m_distortion.at<double>(4, 0) = k3;
			return true;
		}
		else
		{
	//		sprintf(g_error, "error: FILE %s  LINE %d\n", __FILE__, __LINE__);
			return false;
		}
	}
	else
	{
		return false;
	}
}

void CvxCamera::setConfigure(float fx, float fy, float cx, float cy, float k1, float k2, float p1, float p2, float k3)
{
	m_intrinsic   = Mat::eye(3, 3, CV_64F);
	m_distortion  = Mat::zeros(5, 1, CV_64F);

	m_intrinsic.at<double>(0, 0) = fx;
	m_intrinsic.at<double>(1, 1) = fy;
	m_intrinsic.at<double>(0, 2) = cx;
	m_intrinsic.at<double>(1, 2) = cy;

	m_distortion.at<double>(0, 0) = k1;
	m_distortion.at<double>(1, 0) = k2;
	m_distortion.at<double>(2, 0) = p1;
	m_distortion.at<double>(3, 0) = p2;
	m_distortion.at<double>(4, 0) = k3;
}
void CvxCamera::setImageSize(int w, int h)
{
	m_imageSize = cvSize(w, h);
}
double CvxCamera::f(void)
{
	return m_intrinsic.at<double>(0, 0);
}
double CvxCamera::u0(void)
{
	return m_intrinsic.at<double>(0, 2);

}
double CvxCamera::v0(void)
{
	return m_intrinsic.at<double>(1, 2);
}

/************************************************************************/
/* CvxCanonXF105                                                        */
/************************************************************************/

CvxCanonXF105::CvxCanonXF105()
{
	m_maxZoomLevel = 154;
	m_aspectRatio = 1.0;  
	m_apertureWidth  = 4.8;
	m_apertureHeight = 3.6;	
}

void CvxCanonXF105::computerPhysicalParameter()
{
//	sprintf(g_log, "log: FILE %s  LINE %d\n", __FILE__, __LINE__);
	cv::calibrationMatrixValues(m_intrinsic, m_imageSize, m_apertureWidth, m_apertureHeight, 
								m_fovx, m_fovy, m_focalLength, m_principalPoint, m_aspectRatio);
	m_principalPoint.x = m_principalPoint.x * m_imageSize.width / m_apertureWidth;
	m_principalPoint.y = m_principalPoint.y * m_imageSize.height/ m_apertureHeight;
}

void CvxCanonXF105::undistortion(vector<Point2f>& pts)
{
	assert(pts.size() > 0);
	Mat dstMat;
	cv::undistortPoints(Mat(pts), dstMat, m_intrinsic, m_distortion);
	for (int i = 0; i<pts.size(); ++i)
	{
		pts[i].x = dstMat.at<float>(i, 0);
		pts[i].y = dstMat.at<float>(i, 1);
	}

	for (int i = 0; i<pts.size(); ++i)
	{
		pts[i].x = pts[i].x * m_intrinsic.at<double>(0, 0) + m_intrinsic.at<double>(0, 2);
		pts[i].y = pts[i].y * m_intrinsic.at<double>(1, 1) + m_intrinsic.at<double>(1, 2);
	}

}

/************************************************************************/
/* CvxCameraRig                                                         */
/************************************************************************/

void CvxCameraRig::read(FILE *pf)
{
	assert(pf);
	m_leftEye.read(pf);
	m_rightEye.read(pf);
}
void CvxCameraRig::write(FILE *pf)
{
	assert(pf);
	m_leftEye.write(pf);
	m_rightEye.write(pf);
}

bool CvxCameraRig::GetOrientation(const vector<Point3f> &objPts, const vector<Point2f>& leftPts, 
								  const vector<Point2f> &rightPts)
{
	assert(objPts.size() >= 4);
	assert(objPts.size() == leftPts.size() && leftPts.size() == rightPts.size());
	if (objPts.size() != leftPts.size() || leftPts.size() != rightPts.size() ||
		objPts.size() < 4)
	{
	//	sprintf(g_error, "error: FILE %s  LINE %d\n", __FILE__, __LINE__);
		return false;
	}
	vector<vector<Point3f> > objP;
	vector<vector<Point2f> > leftP;
	vector<vector<Point2f> > rightP;
	
	Mat essential;     //3 * 3	 
	Mat fundamental;   //3 * 3
	objP.push_back(objPts);
	leftP.push_back(leftPts);
	rightP.push_back(rightPts);

	//computer extrinsic matrix
	double err = cv::stereoCalibrate(objP, leftP, rightP,
						m_leftEye.m_intrinsic, m_leftEye.m_distortion,
						m_rightEye.m_intrinsic, m_rightEye.m_distortion,
						m_leftEye.m_imageSize, m_rotation, m_translation, essential, fundamental,
						TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 0.01), CALIB_FIX_INTRINSIC);

//	fprintf(stdout, "stereo calib error = %f\n", err);

	//cvx::CvxMat_d d_r(&m_leftEye.m_intrinsic);
	//cvx::CvxMat_d d_t(&m_leftEye.m_distortion);
//	cvx::CvxMat_d d_r(&m_rotation);
//	cvx::CvxMat_d d_t(&m_translation);
	//computer euler angle and translation vector
	Mat tR, tQ;
	m_eulerAngle = cv::RQDecomp3x3(m_rotation, tR, tQ);
		
	float r13, r23, r33;
	r13 = m_rotation.at<double>(0, 2);
	r23 = m_rotation.at<double>(1, 2);
	r33 = m_rotation.at<double>(2, 2);
	float length = sqrt(1.0*r13*r13 + r23*r23 + r33*r33);
	r13 /= length;
	r23 /= length;
	r33 /= length;
	float theta, phi;
	CvxGeometry::GetEulerAngleInSphereCoor(r13, r23, r33, theta, phi);
	theta *= 180/CV_PI;
	if (r13 < 0.0)
	{
		theta = - theta;
	}
//	m_eulerAngle[1] = theta;
//	printf("theta from r13 r23 r33 = %f\n", theta);
	m_translationVec[0] = m_translation.at<double>(0, 0);
	m_translationVec[1] = m_translation.at<double>(1, 0);
	m_translationVec[2] = m_translation.at<double>(2, 0);
	return true;
}

bool CvxCameraRig::GetReprojectionError(const vector<Point3f> &objPts, const vector<Point2f>& leftPts, 
						  const vector<Point2f> &rightPts, double &leftErr, double &rightErr)
{
	assert(objPts.size() >= 4);
	assert(objPts.size() == leftPts.size() && leftPts.size() == rightPts.size());
	if (objPts.size() != leftPts.size() || leftPts.size() != rightPts.size() ||
		objPts.size() < 4)
	{
		return false;
	}

	//left error
	vector<Point2f> projLeftPts(leftPts.size());
	Mat rvec, tvec;
	cv::solvePnP(objPts, leftPts, m_leftEye.m_intrinsic, m_leftEye.m_distortion, rvec, tvec);
	cv::projectPoints(objPts, rvec, tvec, m_leftEye.m_intrinsic, m_leftEye.m_distortion, projLeftPts);
	leftErr = 0.0;
	for (int i = 0; i<leftPts.size(); ++i)
	{
		Point2f p_dif = leftPts[i] - projLeftPts[i];
		leftErr += sqrt(p_dif.x * p_dif.x + p_dif.y * p_dif.y);
	}
	leftErr = leftErr/leftPts.size();

	//right error
	vector<Point2f> projRightPts(rightPts.size());	
	cv::solvePnP(objPts, rightPts, m_rightEye.m_intrinsic, m_rightEye.m_distortion, rvec, tvec);
	cv::projectPoints(objPts, rvec, tvec, m_rightEye.m_intrinsic, m_rightEye.m_distortion, projRightPts);
	rightErr = 0.0;
	for (int i = 0; i<rightPts.size(); ++i)
	{
		Point2f p_dif = rightPts[i] - projRightPts[i];
		rightErr += sqrt(p_dif.x * p_dif.x + p_dif.y * p_dif.y);
	}
	rightErr = rightErr/rightPts.size();
	return true;
}

float CvxCameraRig::GetInterAxis(const vector<Point3f> &objPts, const vector<Point2f>& leftPts, 
								 const vector<Point2f> &rightPts, float unitLength)
{
	assert(objPts.size() >= 4);
	assert(objPts.size() == leftPts.size() && leftPts.size() == rightPts.size());

	//get disparity of first marker center of left and right camera in unit of pixel
	float left_x  = 0;
	float right_x = 0;
	for (int i = 0; i<4; ++i)
	{
		left_x  += leftPts[i].x;
		right_x += rightPts[i].x;
	}
	left_x  /= 4;
	right_x /= 4;
	left_x  -= m_leftEye.m_intrinsic.at<double>(0, 2);
	right_x -= m_rightEye.m_intrinsic.at<double>(0, 2);
	//get average of focal length fy
	float avg_fl = 0.5 * (m_leftEye.m_intrinsic.at<double>(1, 1) + m_rightEye.m_intrinsic.at<double>(1, 1));

	//get average of depth
	float avg_depth = 0;
	{
		double fmc_data[3] = {-unitLength, -unitLength, 0};
		Mat firstMarkerCenter = Mat(3, 1, CV_64F, fmc_data);

		//get left R and T
		Mat leftRvec, leftT, leftRMat;
		cv::solvePnP(objPts, leftPts, m_leftEye.m_intrinsic, m_leftEye.m_distortion, leftRvec, leftT);
		cv::Rodrigues(leftRvec, leftRMat);

		//get right R and T
		Mat rightRvec, rightT, rightRmat;
		cv::solvePnP(objPts, rightPts, m_rightEye.m_intrinsic, m_rightEye.m_distortion, rightRvec, rightT);
		cv::Rodrigues(rightRvec, rightRmat);

		Mat leftfmc3d  = leftRMat * firstMarkerCenter + leftT;
		Mat rightfmc3d = rightRmat * firstMarkerCenter + rightT;
		double dep_left =  leftfmc3d.at<double>(2, 0);
		double dep_right = rightfmc3d.at<double>(2, 0);
		avg_depth = 0.5 * (dep_left + dep_right);
	}

	float ret = avg_depth * (left_x - right_x)/avg_fl;
	return ret;
}

void CvxCameraRig::undistortion(vector<Point2f>& leftPts, vector<Point2f> &rightPts)
{
	assert(leftPts.size() == rightPts.size());
	m_leftEye.undistortion(leftPts);
	m_rightEye.undistortion(rightPts);

}


void cvxAlignment(const vector<cv::Point2f> &leftPts, const vector<Point2f> &rightPts, 
				  const int w, const int h, float &zoomRatio,
				  float &verticalOffsetPercent, float &horizontalOffsetPercent)
{
	assert(leftPts.size() == rightPts.size());
	assert(leftPts.size() == 4);
	assert(w > 0);
	assert(h > 0);

	//computer zoom level;
	float perimeterLeft = 0.0f;
	float perimeterRight = 0.0f;
	for (int i = 0; i<leftPts.size(); ++i)
	{
		Point2f p1 = leftPts[i];
		Point2f p2 = leftPts[(i+1)%leftPts.size()];
		Point2f p_dif = 0.0f;
		p_dif.x = p1.x - p2.x;
		p_dif.y = p1.y - p2.y;
		perimeterLeft += sqrt(double(p_dif.x * p_dif.x + p_dif.y * p_dif.y));
	}

	for (int i = 0; i<rightPts.size(); ++i)
	{
		Point2f p1 = rightPts[i];
		Point2f p2 = rightPts[(i+1)%rightPts.size()];
		Point2f p_dif = 0.0f;
		p_dif.x = p1.x - p2.x;
		p_dif.y = p1.y - p2.y;
		perimeterRight += sqrt(double(p_dif.x * p_dif.x + p_dif.y * p_dif.y));
	}
	zoomRatio = perimeterLeft/perimeterRight*100;
	//computer vertical offset //computer horizontal offset
	float y_dif = 0.0f;
	float x_dif = 0.0f;
	for (int i = 0; i<leftPts.size() && i<rightPts.size(); ++i)
	{
		y_dif += (leftPts[i].y - rightPts[i].y);
		x_dif += (leftPts[i].x - rightPts[i].x);
	}
	y_dif /= leftPts.size();
	x_dif /= leftPts.size();

	verticalOffsetPercent   = y_dif/h*100;
	horizontalOffsetPercent = x_dif/w*100;
}

void cvxAlignRotation(const vector<Point2f> &leftPts, const vector<Point2f> &rightPts, float &rotAngle)
{
	assert(leftPts.size() == rightPts.size());
	assert(leftPts.size() == 4);

	/************************************************************************/
	/*   0  1
		 3  2
	*/        
	/************************************************************************/

	double h1 = leftPts[3].y - leftPts[1].y;
	double w1 = leftPts[1].x - leftPts[3].x;
	double h2 = rightPts[3].y - rightPts[1].y;
	double w2 = rightPts[1].x - rightPts[3].x;
	float a1 = atan2(h1, w1) * 180/CV_PI;
	float a2 = atan2(h2, w2) * 180/CV_PI;
	rotAngle = a1 - a2;
}

void cvxAlignKeyStone(const vector<Point2f> &leftPts, const vector<Point2f> &rightPts, float &ratio)
{
	assert(leftPts.size() == rightPts.size());
	assert(leftPts.size() == 4);

	float l1_left, l2_left;
	{
		float dif_x, dif_y;
		dif_x = leftPts[0].x - leftPts[3].x;
		dif_y = leftPts[0].y - leftPts[3].y;
		l1_left = sqrt(dif_x * dif_x + dif_y * dif_y);

		dif_x = leftPts[1].x - leftPts[2].x;
		dif_y = leftPts[1].y - leftPts[2].y;
		l2_left = sqrt(dif_x * dif_x + dif_y * dif_y);
	}
	float l1_right, l2_right;
	{
		float dif_x, dif_y;
		dif_x = rightPts[0].x - rightPts[3].x;
		dif_y = rightPts[0].y - rightPts[3].y;
		l1_right = sqrt(dif_x * dif_x + dif_y * dif_y);

		dif_x = rightPts[1].x - rightPts[2].x;
		dif_y = rightPts[1].y - rightPts[2].y;
		l2_right = sqrt(dif_x * dif_x + dif_y * dif_y);
	}
	assert(l2_left * l1_right > 0);
	ratio = (l1_left * l2_right)/(l2_left * l1_right) - 1.0;
}

bool cvxFeaturePtsAlignment(const vector<Point2f> &leftPts, const vector<Point2f> &rightPts, 
							const int w, const int h, float &zoomRatio,
							float &verticalOffsetPercent, float &horizontalOffsetPercent, 
							float &rotAngle, vector<int> &marginPtsIdx)
{
	assert(leftPts.size() == rightPts.size());
	assert(w > 0);
	assert(h > 0);

	if (leftPts.size() < 10)
	{
		return false;
	}

	//get four near corner pts;
	vector<Point2f> fourLeft(4);
	vector<Point2f> fourRight(4);
	int minIdx0 = 0;
	int minIdx1 = 1;
	int minIdx2 = 2;
	int minIdx3 = 3;

	//find minimal distance index
	{
		
		int mind0 = INT_MAX;
		int mind1 = INT_MAX;
		int mind2 = INT_MAX;
		int mind3 = INT_MAX;

		for (int i = 0; i<leftPts.size(); ++i)
		{
			int d0, d1, d2, d3;
			Point p(leftPts[i]);
			d0 = p.x * p.x + p.y * p.y;
			d1 = (p.x - w) * (p.x - w) + p.y * p.y;
			d2 = (p.x - w) * (p.x - w) + (p.y - h) * (p.y - h);
			d3 = p.x * p.x + (p.y - h) * (p.y - h);

			if (d0 < mind0)
			{
				mind0 = d0;
				minIdx0 = i;				
			}
			if (d1 < mind1)
			{
				mind1 = d1;
				minIdx1 = i;				
			}
			if (d2 < mind2)
			{
				mind2 = d2;
				minIdx2 = i;				
			}
			if (d3 < mind3)
			{
				mind3 = d3;
				minIdx3 = i;				
			}
		}
	}
	fourLeft[0] = leftPts[minIdx0];
	fourLeft[1] = leftPts[minIdx1];
	fourLeft[2] = leftPts[minIdx2];
	fourLeft[3] = leftPts[minIdx3];

	fourRight[0] = rightPts[minIdx0];
	fourRight[1] = rightPts[minIdx1];
	fourRight[2] = rightPts[minIdx2];
	fourRight[3] = rightPts[minIdx3];
	
	marginPtsIdx.push_back(minIdx0);
	marginPtsIdx.push_back(minIdx1);
	marginPtsIdx.push_back(minIdx2);
	marginPtsIdx.push_back(minIdx3);	

	//computer zoom level;
	{
		float perimeterLeft = 0.0f;
		float perimeterRight = 0.0f;
		for (int i = 0; i<fourLeft.size(); ++i)
		{
			Point2f p1 = fourLeft[i];
			Point2f p2 = fourLeft[(i+1)%fourLeft.size()];
			Point2f p_dif = 0.0f;
			p_dif.x = p1.x - p2.x;
			p_dif.y = p1.y - p2.y;
			perimeterLeft += sqrt(double(p_dif.x * p_dif.x + p_dif.y * p_dif.y));
		}

		for (int i = 0; i<fourRight.size(); ++i)
		{
			Point2f p1 = fourRight[i];
			Point2f p2 = fourRight[(i+1)%fourRight.size()];
			Point2f p_dif = 0.0f;
			p_dif.x = p1.x - p2.x;
			p_dif.y = p1.y - p2.y;
			perimeterRight += sqrt(double(p_dif.x * p_dif.x + p_dif.y * p_dif.y));
		}
		zoomRatio = perimeterLeft/perimeterRight*100;
	}
	//rotation
	cvxAlignRotation(fourLeft, fourRight, rotAngle);

	//vertical horizontal offset
	float y_dif = 0.0f;
	float x_dif = 0.0f;
	for (int i = 0; i<leftPts.size() && i<rightPts.size(); ++i)
	{
		y_dif += (leftPts[i].y - rightPts[i].y);
		x_dif += (leftPts[i].x - rightPts[i].x);
	}
	y_dif /= leftPts.size();
	x_dif /= leftPts.size();

	verticalOffsetPercent   = y_dif/h*100;
	horizontalOffsetPercent = x_dif/w*100;

	return true;
}
